/***************************************************************************
 *   Copyright (C) 2004 by cyril dupuit                                    *
 *   cyrildupuit@hotmail.com                                               *
 *                                                                         *
 *   This program is free software; you can redistribute it and/or modify  *
 *   it under the terms of the GNU General Public License as published by  *
 *   the Free Software Foundation; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   This program is distributed in the hope that it will be useful,       *
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
 *   GNU General Public License for more details.                          *
 *                                                                         *
 *   You should have received a copy of the GNU General Public License     *
 *   along with this program; if not, write to the                         *
 *   Free Software Foundation, Inc.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/

//*****************************************************************************
// Nom du module : math.c
// Description : Librairie mathematique standard ANSI ou presque.
//*****************************************************************************
 
// Historique :
// 28/10/04 : Creation du module V1.0

#include "Lib/StdLib/math.h"

//*****************************************************************************
// Variables globales a ce module
//*****************************************************************************
static double fone = 1.0, fZero[] = {0.0, -0.0,};
static const float one = 1.0, Zero[] = {0.0, -0.0,};

//*****************************************************************************
// Prototypes :
//*****************************************************************************
static float __ieee754_fmodf(float x, float y);
static double __ieee754_fmod(double x, double y);

/*
 * modf(double x, double *iptr)
 * return fraction part of x, and return x's integral part in *iptr.
 * Method:
 *	Bit twiddling.
 *
 * Exception:
 *	No exception.
 */
double modf(double x, double *iptr)
{
	long i0,i1,j0;
	unsigned long i;
	unsigned long high;
	
	EXTRACT_WORDS(i0,i1,x);
	j0 = ((i0 >> 20) & 0x7ff) - 0x3ff;	// exponent of x
	if(j0 < 20)
	{// integer part in high x 
		if(j0 < 0)
		{// |x|<1
	        	INSERT_WORDS(*iptr, i0 & 0x80000000, 0);// *iptr = +-0 
			return x;
		}
		else
		{
			i = (0x000fffff)>>j0;
			if(((i0 & i) | i1) == 0)
			{// x is integral 
				*iptr = x;
				GET_HIGH_WORD(high, x);
				INSERT_WORDS(x, high & 0x80000000, 0);// return +-0 
				return x;
			}
			else
			{
				INSERT_WORDS(*iptr,i0&(~i),0);
				return x - *iptr;
			}
		}
	}
	else if(j0 > 51)
	{// no fraction part
		*iptr = x*fone;
		GET_HIGH_WORD(high,x);
		INSERT_WORDS(x, high & 0x80000000, 0);// return +-0 
		return x;
	}
	else
	{// fraction part in low x 
		i = ((unsigned long)(0xffffffff)) >> (j0 - 20);
		if((i1 & i)==0)
		{/* x is integral */
			*iptr = x;
			GET_HIGH_WORD(high,x);
			INSERT_WORDS(x, high & 0x80000000, 0);// return +-0 
			return x;
		}
		else
		{
			INSERT_WORDS(*iptr, i0, i1 & (~i));
			return x - *iptr;
		}
	}
}


double fmod(double x, double y)	// wrapper fmod 
{
	return __ieee754_fmod(x,y);
}

float fmodf(float x, float y)
{
	return __ieee754_fmodf(x,y);
}

/*
 * __ieee754_fmodf(x,y)
 * Return x mod y in exact arithmetic
 * Method: shift and subtract
 */
static float __ieee754_fmodf(float x, float y)
{
	long n,hx,hy,hz,ix,iy,sx,i;

	GET_FLOAT_WORD(hx,x);
	GET_FLOAT_WORD(hy,y);
	sx = hx & 0x80000000;	// sign of x
	hx ^= sx;		// |x| 
	hy &= 0x7fffffff;	// |y| 

	// purge off exception values
	if((hy == 0)||(hx >= 0x7f800000)|| (hy>0x7f800000))
	{// y=0,or x not finite or y is NaN
		return (x*y)/(x*y);
	}
	if(hx < hy)
	{// |x|<|y| return x
		return x;
	}
	if(hx == hy)
	{// |x|=|y| return x * 0
		return Zero[(unsigned long)sx>>31];	
	}

	//determine ix = ilogb(x)
	if(hx<0x00800000)
	{// subnormal x
		for(ix = -126, i = (hx << 8); i > 0; i <<= 1) ix -=1;
	}
	else ix = (hx >> 23) - 127;

	//determine iy = ilogb(y)
	if(hy<0x00800000)
	{// subnormal y 
		for(iy  = -126, i = (hy << 8); i >= 0; i <<= 1) iy -=1;
	}
	else iy = (hy >> 23) - 127;

	// set up {hx,lx}, {hy,ly} and align y to x 
	if(ix >= -126)
	{
		hx = 0x00800000 | (0x007fffff & hx);
	}
	else
	{//subnormal x, shift x to normal
	    n = -126 - ix;
	    hx = hx << n;
	}
	if(iy >= -126)
	{
		hy = 0x00800000 | (0x007fffff & hy);
	}
	else
	{// subnormal y, shift y to normal
	    n = -126 - iy;
	    hy = hy << n;
	}

	// fix point fmod
	n = ix - iy;
	while(n--)
	{
		hz = hx - hy;
		if(hz < 0)
		{
			hx = hx+hx;
		}
		else
		{
			if(hz==0)
			{//return sign(x) * 0
				return Zero[(unsigned long) sx >> 31];
			}
			hx = hz+hz;
		}
	}
	hz = hx - hy;
	if(hz >= 0)
	{
		hx = hz;
	}

	// convert back to floating value and restore the sign
	if(hx == 0)
	{//return sign(x)*0
		return Zero[(unsigned long)sx >> 31];
	}
	while(hx<0x00800000)
	{//normalize x
	    hx = hx + hx;
	    iy -= 1;
	}
	if(iy >= -126)
	{//normalize output
		hx = ((hx - 0x00800000) | ((iy + 127) << 23));
		SET_FLOAT_WORD(x,hx|sx);
	}
	else
	{//subnormal output
		n = -126 - iy;
		hx >>= n;
		SET_FLOAT_WORD(x, hx | sx);
		x *= one;//create necessary signal 
	}

	return x;//exact output
}

/*
 * __ieee754_fmod(x,y)
 * Return x mod y in exact arithmetic
 * Method: shift and subtract
 */

static double __ieee754_fmod(double x, double y)
{
	long n,hx,hy,hz,ix,iy,sx,i;
	unsigned long lx,ly,lz;

	EXTRACT_WORDS(hx, lx, x);
	EXTRACT_WORDS(hy, ly, y);
	sx = hx & 0x80000000;	// sign of x 
	hx ^= sx;		// |x| 
	hy &= 0x7fffffff;	// |y| 

	// purge off exception values */
	if(((hy | ly) == 0)||(hx >= 0x7ff00000)||((hy | ((ly | -ly) >> 31)) > 0x7ff00000))
	{// y=0,or x not finite or y is NaN 
		return (x * y) / (x * y);
	}
	if(hx <= hy)
	{
		if((hx < hy)||(lx < ly)) return x;	// |x|<|y| return x 
		if(lx == ly) return fZero[(unsigned long)sx >> 31];	// |x|=|y| return x*0
	}

	//determine ix = ilogb(x)
	if(hx < 0x00100000) 
	{//subnormal x 
		if(hx == 0)
		{
			for(ix = -1043, i = lx; i > 0; i <<= 1) ix -=1;
		}
		else
		{
			for(ix = -1022,i = (hx << 11); i > 0; i <<= 1) ix -=1;
		}
	}
	else ix = (hx >> 20) - 1023;

	//determine iy = ilogb(y)
	if(hy < 0x00100000)
	{// subnormal y
		if(hy==0)
		{
			for (iy = -1043, i=ly; i>0; i<<=1) iy -=1;
		}
		else
		{
			for (iy = -1022,i=(hy<<11); i>0; i<<=1) iy -=1;
		}
	}
	else iy = (hy >> 20) - 1023;

	//set up {hx,lx}, {hy,ly} and align y to x
	if(ix >= -1022)
	{
		hx = 0x00100000 | (0x000fffff & hx);
	}
	else
	{//subnormal x, shift x to normal 
		n = -1022-ix;
		if(n <= 31)
		{
			hx = (hx << n)|(lx >> (32 - n));
		lx <<= n;
		}
		else
		{
			hx = lx << (n - 32);
			lx = 0;
		}
	}
	if(iy >= -1022)
	{
		hy = 0x00100000 | (0x000fffff & hy);
	}
	else
	{// subnormal y, shift y to normal
		n = -1022-iy;
		if(n <= 31)
		{
			hy = (hy << n) | (ly >> (32 - n));
			ly <<= n;
		}
		else
		{ 
			hy = ly << (n - 32);
			ly = 0;
		}
	}

	//fix point fmod
	n = ix - iy;
	while(n--)
	{
		hz = hx-hy;
		lz = lx-ly;
		if(lx<ly) hz -= 1;
		if(hz<0)
		{
			hx = hx+hx+(lx>>31);
			lx = lx+lx;
		}
		else
		{
			if((hz | lz) == 0)
			{// return sign(x)*0
				return fZero[(unsigned long)sx >> 31];
			}
			hx = hz + hz + (lz >> 31);
			lx = lz + lz;
		}
	}
	hz = hx - hy;
	lz = lx - ly;
	if(lx < ly) hz -= 1;
	if(hz >= 0)
	{
		hx = hz;
		lx = lz;
	}

	// convert back to floating value and restore the sign
	if((hx | lx) == 0)
	{//return sign(x)*0 
		return fZero[(unsigned long)sx >> 31];
	}
	while(hx < 0x00100000)
	{//normalize x
		hx = hx + hx + (lx >> 31);
		lx = lx + lx;
		iy -= 1;
	}
	if(iy >= -1022)
	{//normalize output 
		hx = ((hx - 0x00100000) | ((iy + 1023) << 20));
		INSERT_WORDS(x, hx | sx, lx);
	}
	else
	{//subnormal output 
		n = -1022 - iy;
		if(n <= 20)
		{
			lx = (lx >> n) | ((unsigned long)hx << (32 - n));
			hx >>= n;
		}
		else if(n<=31)
		{
			lx = (hx << (32 - n)) | (lx >> n);
			hx = sx;
		}
		else
		{
			lx = hx >> (n - 32);
			hx = sx;
		}
		INSERT_WORDS(x,hx|sx,lx);
		x *= fone;// create necessary signal
	}

	return x;//exact output
}

//*****************************************************************************
